function [curvature] = calculate_curvatrue(track)
%UNTITLED 此处显示有关此函数的摘要
%  计算轨迹点曲率
%track 当前需要计算的轨迹曲线


[x,y]=latlon2Cartesian_point_set([track.Lon, track.Lat]);

curvatures = zeros(1, length(x) - 2);%保存轨迹曲率
for i = 2:length(x) - 1
    x1 = x(i - 1); y1 = y(i - 1);
    x2 = x(i); y2 = y(i);
    x3 = x(i + 1); y3 = y(i + 1);
    area = abs(x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2;
    d12 = sqrt((x2 - x1)^2 + (y2 - y1)^2);
    d23 = sqrt((x3 - x2)^2 + (y3 - y2)^2);
    d31 = sqrt((x3 - x1)^2 + (y3 - y1)^2);
    curvatures(i - 1) = 4 * area / (d12 * d23 * d31);
end

curvatures_diff =abs( diff(curvatures));



%散点曲率计算
% 计算一阶导数和二阶导数
dy = gradient(y, x);
d2y = gradient(dy, x);

% 计算曲率
curvature = abs(d2y) ./ (1 + dy.^2).^(3/2);

% 计算曲率变化
delta_curvature = abs(diff(curvature));

% 计算曲率变化的统计量
mean_curvature_change = mean(delta_curvature);
std_curvature_change = std(delta_curvature);

% 可视化曲率
figure;
subplot(2, 1, 1);
plot(1:length(x), curvature, 'b-', 'LineWidth', 2);
title('Curvature of the Trajectory');
xlabel('Position');
ylabel('Curvature');

subplot(2, 1, 2);
plot(1:length(x)-1, delta_curvature, 'r-', 'LineWidth', 2);
title('Curvature Change');
xlabel('Position');
ylabel('Curvature Change');

% 输出曲率变化的统计量
disp(['Mean Curvature Change: ', num2str(mean_curvature_change)]);
disp(['Standard Deviation of Curvature Change: ', num2str(std_curvature_change)]);





end